#include <ros/ros.h>
#include <std_msgs/Float64.h>

std_msgs::Float64 g_velocity;//声明全局变量  速度
std_msgs::Float64 g_force;//声明全局变量 力的大小

void myCallback(const std_msgs::Float64&message_holder)
{
    ROS_INFO("recevied force value is :%f",message_holder.data);
    //打印force value的数值
    g_force.data = message_holder.data;
    //通过全局变量接收消息的数据，供住函数使用
}

int main(int argc ,char **argv)
{
    ros::init (argc,argv,"simulator");
    ros::NodeHandle nh;

    ros::Subscriber my_subscriber_object = nh.subscribe("force_cmd",1,myCallback);
    //订阅力的大小
    ros::Publisher my_publisher_object = nh.advertise<std_msgs::Float64>("velocity",1);
    //发布速度的大小

    double mass = 1.0 ; //物体的质量
    double dt = 0.1 ;//定义积分的时间间隔为0.1s
    double sample_rate = 1.0/dt ;//计算更新频率
    
    ros::Rate naptime(sample_rate);//实例化更新频率对象

    g_velocity.data = 0.0;//初始速度为0  
    g_force.data = 0.0;//初始力为0  但是这是一个全局变量，随着回调函数，及时更新

    while(ros::ok())
    {
        g_velocity.data = g_velocity.data + (g_force.data / mass) * dt;
        //  v = v + at 加速度的欧拉积分

        my_publisher_object.publish(g_velocity);
        //发布系统的状态， 也就是速度数值

        ROS_INFO("velocity = %f",g_velocity.data);
        //打印出速度的数值

        ros::spinOnce();
        //允许从回调函数更新数据

        naptime.sleep();
        //等待指定时间的剩余时间，
        //指定控制器的更新频率 10 hz
        //然而，不管这么样。模拟器都是10ms前进一次
        
    }

    return 0;
    //和之前的一样，
    //只要存在回调函数，
    //就不会执行这里，
    //除非ros崩溃

}
